from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import Command, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch.conditions import IfCondition
import os


# 启动指令： ros2 launch urdf_pkg display.launch.py urdf_file:=my_robot.urdf use_rviz:=false
def generate_launch_description():
    # 声明用于配置的 launch 参数
    urdf_file_arg = DeclareLaunchArgument(
        name='urdf_file',
        default_value='simple_robot.urdf',
        description='URDF/XACRO 文件名'
    )
    
    use_gui_arg = DeclareLaunchArgument(
        name='use_gui',
        default_value='true',
        description='是否启动 joint_state_publisher_gui'
    )
    
    use_rviz_arg = DeclareLaunchArgument(
        name='use_rviz',
        default_value='true',
        description='是否启动 RViz2'
    )
    
    rviz_config_arg = DeclareLaunchArgument(
        name='rviz_config',
        default_value='view_robot.rviz',
        description='RViz 配置文件名称'
    )
    
    # 获取参数值
    urdf_file = LaunchConfiguration('urdf_file')
    use_gui = LaunchConfiguration('use_gui')
    use_rviz = LaunchConfiguration('use_rviz')
    rviz_config = LaunchConfiguration('rviz_config')
    
    # 获取包路径
    pkg_share = FindPackageShare('urdf_pkg')
    
    # 构建 URDF 文件路径
    urdf_path = PathJoinSubstitution([
        pkg_share, 
        'urdf', 
        urdf_file
    ])
    
    # 构建 RViz 配置文件路径
    rviz_config_path = PathJoinSubstitution([
        pkg_share, 
        'rviz', 
        rviz_config
    ])
    
    # 加载 URDF 模型
    robot_description = {
        #通过xacro命令处理urdf文件
        'robot_description': Command(['xacro ', urdf_path])
    }
    
    # 启动 robot_state_publisher 节点
        #1. 节点启动时，会读取parameters中传入的robot_description参数（里面包含 URDF 模型数据）
        #2. 解析 URDF 中定义的机器人结构（连杆、关节类型和连接关系）
        #3. 监听/joint_states话题（通常由joint_state_publisher提供）获取关节角度信息
        #4. 根据关节角度和 URDF 中的运动学模型，实时计算每个连杆相对于基座的位姿
        #5. 通过/tf和/tf_static话题发布这些坐标变换
    robot_state_publisher_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        output='screen',
        parameters=[robot_description]
    )
    
    # 启动 joint_state_publisher_gui 或 joint_state_publisher
    joint_state_publisher_gui_node = Node(
        package='joint_state_publisher_gui',
        executable='joint_state_publisher_gui',
        name='joint_state_publisher_gui',
        output='screen',
        condition=IfCondition(use_gui)
    )
    
    # 启动 RViz2 节点
        #1. 启动后，RViz2 会读取指定的配置文件（如view_robot.rviz）
        #2. 自动订阅配置中指定的 ROS 话题（如/tf变换、/robot_description模型）
        #3. 在 3D 视图中渲染机器人模型，并根据实时接收的关节状态和 TF 变换更新模型姿态
        #4. 允许用户交互式调整视角、添加 / 删除可视化组件、保存新的配置等
    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        arguments=['-d', rviz_config_path],
        condition=IfCondition(use_rviz)
    )
    
    return LaunchDescription([
        urdf_file_arg,
        use_gui_arg,
        use_rviz_arg,
        rviz_config_arg,
        robot_state_publisher_node,
        joint_state_publisher_gui_node,
        rviz_node
    ])